#include "../User/Task/Control.h"

#include "../User/Device/Control.h"
#include "cmsis_os.h"
#include "main.h"
#include "sys.h"

/* USER CODE BEGIN Header_Task_Control */
/**
 * @brief Function implementing the TaskControl thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_Task_Control */
void Task_Control(void *argument) {
  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_CONTROL_SCAN_FREQ;

  Control_Init();

  uint32_t tick = osKernelGetTickCount();

  while (1) {
    tick += delay_tick;

    Control_CalOutput(&(Task_Runtime.pid.angle), &(Task_Runtime.pid.speed),
                      &Task_Runtime.angle, &Task_Runtime.gyro,
                      &Task_Runtime.cmd_param);

    osDelayUntil(tick);
  }
}
